//
// Argon v0.1 - March 2009
// The Arduino Quadrocopter
// Ville Vilpas (Psoden)
//
#include <ServoTimer2.h>
#include <PID_Beta6.h>

// Allow << streaming
template<class T> inline Print &operator <<(Print &obj, T arg) { 
  obj.print(arg); 
  return obj; 
}

//*********** Initialize variables ***********//
#define DEBUG 0

// XBee
#define BAUD 115200
int inByte;
boolean firstContact = false;
union {
  byte b[4];
  long asLong;
} 
outBuffer; 
union {
  byte b[4];
  long asLong;
} 
inBuffer; 

// Control
long txCommands[4];
#define ROLL 0
#define PITCH 1
#define YAW 2
#define THROTTLE 3

// PID
#define P 0
#define I 1
#define D 2
#define SAMPLETIME 10
double planeGain[3] = {2,5,1};
double yawGain[3] = {2,5,1};
double pSetPoint[3], pInput[3], pOutput[3];
PID rollPID(&pInput[ROLL], &pOutput[ROLL], &pSetPoint[ROLL], planeGain[P], planeGain[I], planeGain[D]);
PID pitchPID(&pInput[PITCH], &pOutput[PITCH], &pSetPoint[PITCH], planeGain[P], planeGain[I], planeGain[D]);
PID yawPID(&pInput[YAW], &pOutput[YAW], &pSetPoint[YAW], yawGain[P], yawGain[I], yawGain[D]);

// Motors
#define MINCOMMAND 1000
#define MAXCOMMAND 2000
#define FRONT 0
#define REAR 1
#define RIGHT 2
#define LEFT 3
#define FRONTMOTORPIN 7
#define REARMOTORPIN 8
#define RIGHTMOTORPIN 9
#define LEFTMOTORPIN 10
ServoTimer2 frontMotor;
ServoTimer2 rearMotor;
ServoTimer2 rightMotor;
ServoTimer2 leftMotor;
int motorCommand[4];

// Sensors
// Smoothing parameters
int rollSmoothArray [16];
int pitchSmoothArray [16];
int yawSmoothArray [16];
#define ZEROLIMIT 5
int rollGyroZero, pitchGyroZero, yawGyroZero;
int rollRate, pitchRate, yawRate;

// Scale gyro rate (-465 to +465) to motor commands (1000 to 2000)
// use y = mx + b
// m = (y2 - y1) / (x2 - x1) = (2000 - 1000) / (465 - (-465))
float mMotorRate = 1.0753;
// b = y1 - m * x1
float bMotorRate = 1500;

// Timing
int loopRate, previousRate, currentRate;
//************* Setup *************//
void setup() {
  Serial.begin(BAUD);
  //analogReference(EXTERNAL); // 3.3v
  
  frontMotor.attach(FRONTMOTORPIN);
  rearMotor.attach(REARMOTORPIN);
  rightMotor.attach(RIGHTMOTORPIN);
  leftMotor.attach(LEFTMOTORPIN);
  commandAllMotors(MINCOMMAND);
  
  zeroGyros();
  
  initPID();
  
  establishContact();
}

//*********** Main loop ***********//
void loop() {
  if(Serial.available() > 16) {
    respondSerial();
  }
  
  // Read Sensors
  // Average sensor values and force them to be centered at 0
  pInput[ROLL] = (mMotorRate * (smooth(analogRead(ROLL), rollSmoothArray) - rollGyroZero)) + bMotorRate;
  pInput[PITCH] = (mMotorRate * (smooth(analogRead(PITCH), pitchSmoothArray) - pitchGyroZero)) + bMotorRate;
  pInput[YAW] = (mMotorRate * (smooth(analogRead(YAW), yawSmoothArray) - yawGyroZero)) + bMotorRate;
  
  pSetPoint[ROLL] = txCommands[ROLL];
  pSetPoint[PITCH] = txCommands[PITCH];
  pSetPoint[YAW] = txCommands[YAW];
  
  computePID();
  
  motorCommand[FRONT] = constrain((txCommands[THROTTLE] - pOutput[PITCH] + pOutput[YAW]), MINCOMMAND, MAXCOMMAND);
  motorCommand[REAR] = constrain((txCommands[THROTTLE] + pOutput[PITCH] + pOutput[YAW]), MINCOMMAND, MAXCOMMAND);
  motorCommand[RIGHT] = constrain((txCommands[THROTTLE] - pOutput[ROLL] - pOutput[YAW]), MINCOMMAND, MAXCOMMAND);
  motorCommand[LEFT] = constrain((txCommands[THROTTLE] + pOutput[PITCH] - pOutput[YAW]), MINCOMMAND, MAXCOMMAND);
    
  commandMotors();
  
  // Timing
  currentRate = millis();
  loopRate = currentRate - previousRate;
  if(loopRate > 250) { loopRate = 250; }
  previousRate = currentRate;
}
